// $Id: ids.c 52 2012-07-18 22:03:13Z alan.watson.f $

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <time.h>
#include <math.h>

#include "oaxaca-config.h"
#include "oaxaca.h"
#include "ids.h"
#include "mat.h"

#include <sys/time.h>
#include <unistd.h>

#include <ueye.h>

static HIDS camera;

void
ueye_error(const char *activity)
{
  INT code;
  IS_CHAR *text;
  if (is_GetError(camera, &code, &text) != IS_SUCCESS)
    oaxaca_error("ueye error while %s.", activity);
  else
    oaxaca_error("ueye error while %s: %s.", activity, text);
}

static void
getpixelclockfrequency(double *frequency)
{
  oaxaca_info("getting pixel clock frequency.");
  UINT u; 
  if (is_PixelClock(camera, IS_PIXELCLOCK_CMD_GET, &u, sizeof(u)) != IS_SUCCESS)
    ueye_error("getting pixel clock frequency");
  oaxaca_info("pixel clock frequency is %u MHz.", u);
  if (frequency != NULL)
    *frequency = u * 1e6;
}

static void
setpixelclockfrequency(double frequency)
{
  oaxaca_info("setting pixel clock frequency.");
  UINT u = round(frequency / 1e6);
  if (is_PixelClock(camera, IS_PIXELCLOCK_CMD_SET, &u, sizeof(u)) != IS_SUCCESS)
    ueye_error("setting pixel clock frequency");
  getpixelclockfrequency(NULL);
}

static void
setframefrequency(double frequency)
{
  oaxaca_info("setting frame frequency to %.0f Hz.", frequency);
  double x;
  if (is_SetFrameRate(camera, frequency, &x) != IS_SUCCESS)
    ueye_error("setting frame frequency");
  oaxaca_info("frame frequency is %.0f Hz.", x);
  x = 0;
  if (is_Exposure(camera, IS_EXPOSURE_CMD_SET_EXPOSURE, &x, sizeof(x)) != IS_SUCCESS)
    ueye_error("setting exposure time");
  if (is_Exposure(camera, IS_EXPOSURE_CMD_GET_EXPOSURE, &x, sizeof(x)) != IS_SUCCESS)
    ueye_error("getting exposure time");
  oaxaca_info("exposure time is %.6f s.", x * 1e-3);
}

static void
getframefrequency(double *frequency)
{
  oaxaca_info("getting frame frequency.");
  double x;
  if (is_SetFrameRate(camera, IS_GET_FRAMERATE, &x) != IS_SUCCESS)
    ueye_error("getting frame frequency");
  oaxaca_info("frame frequency is %.0f Hz.", x);
  if (frequency != NULL)
    *frequency = x;
}

static void
getframefrequencyrange(double *minfrequency, double *maxfrequency)
{
  oaxaca_info("getting frame frequency range.");
  double x0, x1, x2;
  if (is_GetFrameTimeRange(camera, &x0, &x1, &x2) != IS_SUCCESS)
    ueye_error("getting frame time range");
  oaxaca_info("minimum frame time is %.6f s.", x0);
  oaxaca_info("maximum frame time is %.6f s.", x1);
  oaxaca_info("frame time increment is %.6f s.", x2);
  oaxaca_info("minimum frame frequency is %.0f Hz.", 1.0 / x1);
  oaxaca_info("maximum frame frequency is %.0f Hz.", 1.0 / x0);
  if (minfrequency != NULL)
    *minfrequency = 1.0 / x1;
  if (maxfrequency != NULL)
    *maxfrequency = 1.0 / x0;
}

static void
setwindow(int x, int y, int nx, int ny)
{
  oaxaca_info("setting window.");
  IS_RECT r;
  r.s32X      = x;
  r.s32Y      = y;
  r.s32Width  = nx;
  r.s32Height = ny;
  if (is_AOI(camera, IS_AOI_IMAGE_SET_AOI, &r, sizeof(r)) != IS_SUCCESS)
    ueye_error("setting window");
  double maxfrequency;
  getframefrequencyrange(NULL, &maxfrequency);
  setframefrequency(maxfrequency);
  getframefrequency(NULL);
}

static void
getwindow(int *x, int *y, int *nx, int *ny)
{
  oaxaca_info("getting window.");
  IS_RECT r;
  if (is_AOI(camera, IS_AOI_IMAGE_GET_AOI, &r, sizeof(r)) != IS_SUCCESS)
    ueye_error("getting window");
  oaxaca_info("window origin is (%d,%d).", (int) r.s32X, (int) r.s32Y);
  oaxaca_info("window size is (%d,%d).", (int) r.s32Width, (int) r.s32Height);
  if (x != NULL)
    *x = r.s32X;
  if (y != NULL)
    *y = r.s32Y;
  if (nx != NULL)
    *nx = r.s32Width;
  if (ny != NULL)
    *ny = r.s32Height;
}

void
oaxaca_ids_initialize(int id)
{
  camera = id;
  oaxaca_info("waiting to open detector.");
  sleep(10);
  oaxaca_info("opening detector %d.", id);
  int i = is_InitCamera(&camera, NULL);
  if (i == IS_STARTER_FW_UPLOAD_NEEDED) {
    oaxaca_info("uploading firmware.");
    camera |= IS_ALLOW_STARTER_FW_UPLOAD;
    i = is_InitCamera(&camera, NULL);;
  }
  if (i != IS_SUCCESS)
    ueye_error("opening detector");
  oaxaca_info("camera is %d.", (int) camera);
  oaxaca_info("checking display mode.");
  int dm = is_SetDisplayMode(camera, IS_GET_DISPLAY_MODE);
  if (dm != IS_SET_DM_DIB)
    oaxaca_error("display mode is not IS_SET_DM_DIB.");
  oaxaca_info("setting color mode to 16-bit raw.");
  if (is_SetColorMode(camera, IS_CM_SENSOR_RAW16) != IS_SUCCESS)
    ueye_error("setting color mode");  
  oaxaca_info("setting gain boost to off.");
  if (is_SetGainBoost(camera, IS_SET_GAINBOOST_OFF) != IS_SUCCESS)
    ueye_error("setting gain boost");  
  oaxaca_info("setting hardware gain.");
  if (is_SetHardwareGain(camera, 0, 0, 0, 0) != IS_SUCCESS)
    ueye_error("setting hardware gain"); 
  oaxaca_ids_configure();
  oaxaca_info("finished initializing.");
}

void
oaxaca_ids_configure(void)
{
  oaxaca_info("configuring.");

  setpixelclockfrequency(oaxaca_shm->wfs.pixelclockfrequency);
  getpixelclockfrequency(&oaxaca_shm->wfs.pixelclockfrequency);
  
  setwindow(oaxaca_shm->wfs.window.sx, oaxaca_shm->wfs.window.sy,
            oaxaca_shm->wfs.window.nx, oaxaca_shm->wfs.window.ny);
  getwindow(&oaxaca_shm->wfs.window.sx, &oaxaca_shm->wfs.window.sy,
            &oaxaca_shm->wfs.window.nx, &oaxaca_shm->wfs.window.ny);
     
  getframefrequencyrange(&oaxaca_shm->wfs.minframefrequency,    
                         &oaxaca_shm->wfs.maxframefrequency);

  double framefrequency = oaxaca_shm->wfs.framefrequency;
  if (framefrequency < oaxaca_shm->wfs.minframefrequency)
    framefrequency = oaxaca_shm->wfs.minframefrequency;
  if (framefrequency > oaxaca_shm->wfs.maxframefrequency)
    framefrequency = oaxaca_shm->wfs.maxframefrequency;
  setframefrequency(framefrequency);
  getframefrequency(&oaxaca_shm->wfs.framefrequency);

  oaxaca_info("finished configuring.");
}

void
oaxaca_ids_run(int (*frame_handler)(mat_t *))
{  
  int nx, ny;
  getwindow(NULL, NULL, &nx, &ny);

  int nbuffers = 16;
  char *membuf[nbuffers];
  INT memid[nbuffers];

  oaxaca_info("preparing to acquire frames.");
  for (int i = 0; i < nbuffers; ++i) {
    if (is_AllocImageMem(camera, nx, ny, 16, &membuf[i], &memid[i]) != IS_SUCCESS)
      ueye_error("allocating image memory");
    if (is_AddToSequence(camera, membuf[i], memid[i]) != IS_SUCCESS)
      ueye_error("adding image memory to sequence");
  }
  mat_t *data = mat_alloc(ny, nx);
  INT lineincrement;
  if (is_GetImageMemPitch(camera, &lineincrement) != IS_SUCCESS)
      ueye_error("determining line increment"); 
  if (is_CaptureVideo(camera, IS_WAIT) != IS_SUCCESS)
    ueye_error("acquiring frames");
  is_EnableEvent(camera, IS_SET_EVENT_FRAME);

  oaxaca_info("acquiring frames.");
  for (;;) {
    if (is_WaitEvent(camera, IS_SET_EVENT_FRAME, 1000) == IS_SUCCESS) {
      char *lastmembuf;
      if (is_GetActSeqBuf(camera, NULL, NULL, &lastmembuf) != IS_SUCCESS)
         ueye_error("getting image memory");
      int i;
      for (i = 0; i < nbuffers; ++i) {
        if (lastmembuf == membuf[i])
          break;
      }
      if (i == nbuffers)
        oaxaca_error("unexpected last memory buffer pointer.");
      for (int iy = 0; iy < ny; ++iy) {
        unsigned short *linebuf = (unsigned short*) (lastmembuf + iy * lineincrement);
        for (int ix = 0; ix < nx; ++ix)
          mat_elt(data, iy, ix) = (double) linebuf[ix];
        for (int ix = 0; ix < nx / 2; ++ix)
          mat_elt(data, iy, ix + nx / 2) -= mat_elt(data, iy, ix);
      }
      if (frame_handler(data) == 0)
        break;
    }
  }
  oaxaca_info("finished acquiring frames.");

  is_DisableEvent(camera, IS_SET_EVENT_FRAME);  
  if (is_StopLiveVideo(camera, IS_WAIT) != IS_SUCCESS)
    ueye_error("stopping acquiring frames");
  for (int i = 0; i < nbuffers; ++i)
    if (is_FreeImageMem(camera, membuf[i], memid[i]) != IS_SUCCESS)
      ueye_error("while freeing image memory");
  mat_free(data);
  
}
